﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace AStartTest
{
    public class AST
    {
        Map2D R;

        //开启列表
        List<PointAS> Open_List = new List<PointAS>();

        //关闭列表
        List<PointAS> Close_List = new List<PointAS>();

        //从开启列表查找F值最小的节点
        private PointAS GetMinFFromOpenList()
        {
            PointAS Pmin = null;
            foreach (PointAS p in Open_List) if (Pmin == null || Pmin.G + Pmin.H > p.G + p.H) Pmin = p;
            return Pmin;
        }

        //判断一个点是否为障碍物
        private bool IsBar(PointAS p, byte[,] map)
        {
            if (map[p.y, p.x] == 0) return true;
            return false;
        }

        //判断关闭列表是否包含一个坐标的点
        private bool IsInCloseList(int x, int y)
        {
            foreach (PointAS p in Close_List) if (p.x == x && p.y == y) return true;
            return false;
        }
        //从关闭列表返回对应坐标的点
        private PointAS GetPointFromCloseList(int x, int y)
        {
            foreach (PointAS p in Close_List) if (p.x == x && p.y == y) return p;
            return null;
        }

        //判断开启列表是否包含一个坐标的点
        private bool IsInOpenList(int x, int y)
        {
            foreach (PointAS p in Open_List) if (p.x == x && p.y == y) return true;
            return false;
        }
        //从开启列表返回对应坐标的点
        private PointAS GetPointFromOpenList(int x, int y)
        {
            foreach (PointAS p in Open_List) if (p.x == x && p.y == y) return p;
            return null;
        }


        //计算某个点的G值
        private int GetG(PointAS p)
        {
            if (p.father == null) return 0;
            if (p.x == p.father.x || p.y == p.father.y) return p.father.G + 10;
            else return p.father.G + 14;
        }

        //计算某个点的H值
        private int GetH(PointAS p, PointAS pb)
        {
            return Math.Abs(p.x - pb.x) + Math.Abs(p.y - pb.y);
        }

        //检查当前节点附近的节点
        private void CheckP8(PointAS p0, Map2D map, PointAS pa, ref PointAS pb)
        {
            for (int xt = p0.x - 1; xt <= p0.x + 1; xt++)
            {
                for (int yt = p0.y - 1; yt <= p0.y + 1; yt++)
                {
                    //排除超过边界和等于自身的点
                    if ((xt >= 0 && xt < R.Col && yt >= 0 && yt < R.Row) 
                        && !(xt == p0.x && yt == p0.y))
                    {
                        // 排除跳过2个不能通过的格子，斜穿
                        //排除障碍点和关闭列表中的点
                        if ((map.GetCell(new MapPos(yt, xt)).Value == 0 && NotCross(yt,xt, p0, map))
                            && !IsInCloseList(xt, yt))
                        {
                            if (IsInOpenList(xt, yt))
                            {
                                PointAS pt = GetPointFromOpenList(xt, yt);
                                int G_new = 0;
                                if (p0.x == pt.x || p0.y == pt.y) G_new = p0.G + 10;
                                else G_new = p0.G + 14;
                                if (G_new < pt.G)
                                {
                                    Open_List.Remove(pt);
                                    pt.father = p0;
                                    pt.G = G_new;
                                    Open_List.Add(pt);
                                }
                            }
                            else
                            {
                                //不在开启列表中
                                PointAS pt = new PointAS();
                                pt.x = xt;
                                pt.y = yt;
                                pt.father = p0;
                                pt.G = GetG(pt);
                                pt.H = GetH(pt, pb);
                                Open_List.Add(pt);
                            }
                        }
                    }
                }
            }
        }

        private bool NotCross(int yt, int xt, PointAS p0, Map2D map)
        {
            if (Math.Abs(yt - p0.y) + Math.Abs(xt - p0.x) == 2)
            {
                if (map.GetCell(new MapPos(p0.y, xt)).Value == 0
                    && map.GetCell(new MapPos(yt, p0.x)).Value == 0)
                {
                    return true;
                }
                return false;
            }
            else
            {
                return true;
            }
        }

        public List<MapPos> FindWay(PointAS pa, PointAS pb, Map2D map)
        {
            List<MapPos> path = new List<MapPos>();

            R = map;
            Open_List.Add(pa);
            while (!(IsInOpenList(pb.x, pb.y) || Open_List.Count == 0))
            {
                PointAS p0 = GetMinFFromOpenList();
                if (p0 == null)
                {
                    return null;
                }
                Open_List.Remove(p0);
                Close_List.Add(p0);
                CheckP8(p0, R, pa, ref pb);
            }

            PointAS p = GetPointFromOpenList(pb.x, pb.y);
            path.Add(new MapPos(p.y, p.x));
            while (p.father != null)
            {
                path.Insert(0, new MapPos(p.father.y, p.father.x));
                p = p.father;
            }

            return path;
        }
    }

    public class PointAS
    {
        public int y;
        public int x;
        public int G;
        public int H;

        public PointAS()
        {
        }
        public PointAS(int x0, int y0, int G0, int H0, PointAS F)
        {
            x = x0;
            y = y0;
            G = G0;
            H = H0;
            father = F;
        }

        public PointAS father;
    }
}
